DYNAMIC ATTITUDE MEASUREMENT 
METHOD AND APPARATUS 

Related Applications: 

[0001] This application is a continuation of Application Ser. No. 10/136,955, 
entitled "DYNAMIC ATTITUDE MEASUREMENT METHOD AND 
APPARATUS", filed on May 1, 2002 by Michael Horton, which application is a 
continuation-in-part of Application Ser. No. 09/326,738, entitled "DYNAMIC 
ATTITUDE MEASUREMENT SENSOR AND METHOD", filed on June 4, 1999 
by M. Horton, et al., which claims priority from U.S. Provisional Application Ser. 
No. 60/088,160, filed on June 5, 1998, by Michael A. Horton, entitled 
"DYNAMIC ATTITUDE MEASUREMENT SENSOR AND METHOD", which 
applications are fully incorporated herein by reference. 

Field of the Invention: 

[00021 This invention relates to the measurement of attitude of accelerating 
bodies, and more particularly to attitude sensors and control systems. 

Background of the Invention: 

[0003] Certain known techniques for measuring and controlling attitude of a 
moving body commonly rely upon a gyroscope spinning about a vertical axis, or 
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upon a liquid sensor, or upon a pendulum device responsive to downward 
gravitational orientation. However, the mass of a pendulum renders such devices 
also responsive to acceleration, and hence are not useful as attitude sensors in 
dynamic systems involving an accelerating body. Similarly, liquid sensors exhibit 
mass that affects attitude detection during acceleration, and also such sensors are 
vulnerable to vibration that affects surface characteristics of the liquid upon which 
sensing may depend. 

[0004] Vertically-oriented spinning gyroscopes commonly operate as attitude 
sensors, but are usually heavy, bulky, expensive devices that are subject to 
precession and drift errors. They also suffer from poor reliability in rugged 
operating environments due to the moving parts that make up the technology 
which require periodic maintenance to keep the units operational. 
[0005] Other known attitude sensors rely upon multiple GPS receivers at 
spaced locations to compute attitude from signals received at each location. 
However, such computation of attitude is subject to the distance inaccuracy of 
signals received at each location, and the spacing of the locations should be very 
much larger than the distance error associated with each such location, and this 
contributes to unacceptably large systems for making fine attitude measurements. 
Rate sensors of negligible mass such as ring laser gyroscopes have been used in 
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attitude-sensing measurements, but are vulnerable to drift and associated long-term 
instability. 

Summary of the Invention: 

[0006] In accordance with the present invention, accurate attitude sensing is 
accomplished by measuring acceleration in three orthogonal axes and measuring 
angular rate about each such axis to compute attitude accurately relative to a 
vertical axis. Solid-state accelerometers and rate sensors are temperature 
compensated and are assembled into a small common housing for applications in 
rugged environments. Measurement errors attributable to fabrication 
misaUgnments, and the like, are calibrated out following initial assembly for highly 
reliable and accurate outputs from a compact, rugged assembly of components. 
Vibrating ceramic plates operate as rate sensors responsive to Coriolis forces to 
produce angular rate outputs independently of acceleration, and micromachined 
silicon devices operating as differential capacitors to sense acceleration in aligned 
directions independently of angular rate about the orthogonal axes. 
[0007] A method in accordance with the present invention includes converting 
analog outputs of all sensors to digital values with stored calibration correction 
values. The signals representing total angular rate or rotational velocity about each 
of the orthogonal axes is integrated into a quaternion (i.e., a 4-element vector that 
completely describes the orientation of an object), and total angular rate is 
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computed from the sensed and corrected rates. The direction cosines are computed 
from the quaternion, and the accelerations of the assembly are computed from the 
three acceleration signals multiplied by the direction cosines. Attitude and other 
parameters or orientation and motion of the assembly are derived from the data 
produced by the accelerometers and rate sensors within the common assembly. 
[0008] In one embodiment of the present invention, a relatively inexpensive 
and compact-sized system generates an accurate representation of an attitude of an 
object. In particular, the system measures attitude for an object that may be 
accelerating, thereby overcoming the drawbacks of conventional attitude 
measuring devices mentioned above. Furthermore, such embodiment of the 
present invention provides a self-tuning system that automatically compensates for 
drift and that automatically updates the quatemion obtained from sensor outputs. 
The system generates highly accurate output data based upon measurements 
obtained from commercially available, low or mid-level performance sensors. In 
addition, the user of the system can provide input commands that can adjust the 
output data of the system in order to fiirther compensate for factors in the 
enviroimient of the system. This embodiment of the present invention also reduces 
the manufacturing complexity of attitude measurement devices by providing a 
calibration sequence that reduces Ihe number of testing steps during the 
manufacturing process. 
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[0009] In another embodiment of the present invention, improved algorithms 
for attitude and heading calculations are based upon extended Kalman filter 
trajectory corrections, with accelerometers providing attitude reference 
information, and the Kalman filter providing corrections to the attitude trajectory 
as calculated fi-om integration of rate sensor information. Extended Kalman filter 
algorithm involves intense calculations performed by a dedicated processor. A 
master processor calculates the attitude trajectory, and a dedicated or slave 
processor calculates the Kalman filter corrections and estimates of sensor bias. 
Resultant attitude errors are less than . 1 degree under static conditions, and are a 
function of the dynamic acceleration profile under dynamic conditions. Typical 
results obtained under flight test conditions analogous to light-aerobatic maneuvers 
indicated only about 1-2 degrees RMS attitude errors. 

Description of the Drawings: 

[0010] Figure 1 is a graph illustrating coordinate axes with respect to which the 
present invention responds to movements and orientations; 
[0011] Figure 2 is a block schematic diagram of one embodiment of the 
present invention; 

[0012] Figure 3 is a flow chart of an operational sequence according to the 
present invention; 



5 



1 8856/08206/DOCS/l 383593. 1 



[0013] Figure 4A is a schematic diagram of one embodiment of the present 
invention for enhancing bandwidth; 

[0014] Figure 4B is a graph showing bandwidth compensation according to the 
present invention;, and 

[0015] Figure 5A is a graph illustrating error correction over time under 
proportional gain control; 

[0016] Figure 5B is a graph illustrating error correction over time imder fixed 

rate gain control according to the present invention; 

[0017] Figure 6 is a flowchart describing a method for determining the 

calibration coefficients and correction factors thereby providing a "factory 

calibration" used in accordance with the present invention; 

[0018] Figure 7 is a flowchart illustrating additional details of the quaternion 

update and normalization; 

[0019] Figure 8 is a schematic diagram of a system including co-processors for 

computing attitude trajectory and Kalman filter corrections; 

[0020] Figure 9 is a block schematic diagram of progressive stages of 

compensation of data suitable for extended Kalman filter processing; 

[0021] Figure 10 is a graph showing the vectors involved in calculating attitude 

error corrections associated with the body fi-ame of a vehicle in relation to a 

tangent frame or local level horizontal fi"ame; 
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[0022] Figure 1 1 is a flow chart illustrating one component of an attitude error 
decision model according to the present invention; and 

[0023] Figure 12 is a flow chart illustrating another component of an attitude 
error decision model. 

Detailed Description of the Invention: 

[0024] Referring now to the graph of Figure 1, there is shown a coordinate set 
of axes X, Y, and Z that designate directions of movement or orientations with 
respect to the 'horizon' 8 as a reference plane, and about which rotational motions 
are specifically legend as 'pitch' (i.e., rotation about Y in the XZ plane), and 'roll' 
(i.e., rotation about X in the YZ plane), and 'yaw' (i.e., rotation about Z in the X Y 
plane, also referenced as 'heading' or 'azimuth'). 

[0025] In accordance with the present invention, as illustrated in the block 
diagram of Figure 2, solid-state accelerometers 9, 10, 11 are disposed in alignment 
with each of the X, Y, and Z axes, respectively, and inertial elements 13, 14, 15 are 
disposed to sense rate of change of angular displacement about each of the X, Y, 
and Z axes, respectively. The accelerometers 9-1 1 may each comprise 
conventional micro-machined sihcon devices that operate on differential 
capacitance to produce an analog output indication of axial acceleration. Suitable 
devices are commercially available as Model No. ADXL05 from Analog Devices, 
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Inc. of Norwood, Massachusetts. Similarly, the inertial elements 13-15 are solid- 
state devices that each comprises a vibrating ceramic plate responsive to Coriolis 
force to produce an analog output indication of angular rate independent of 
acceleration. Suitable devices are commercially available as Model No. ENV- 
05H-2 from Murata Manufacturing Co., Ltd. in Japan. The accelerometers and 
rate sensors and a temperature sensor 17 may be assembled within a confining 
housing of about 3 cubic inches, or less, for unobtrusive installation in numerous 
applications. Optional magnetic sensors 18a- 18c may be aligned with each of the 
X, Y, and Z axes in order to provide correction factors for heading (yaw), as 
described below. The analog outputs from the accelerometers 9-11, and from the 
inertial elements 13-15, and from the temperature-sensing element 17, and from 
magnetic sensors 18a- 18c, are supplied as outputs 19 of the assembly, and are also 
supplied to an on-board Analog-to-Digital (A/D) converter 21 for conversion in 
time-share mode to digital data with a minimum 12-bit accuracy with respect to the 
apphed analog input signals. The digitized data is supplied to an on-board 
processor 23 having Electrically Erasable Programmable Read-Only Memory 
(EEPROM) 24 with storage locations therein for storing the calibration data for the 
sensors, as later described herein. The processor 23 also includes a frequency 
compensation network 26 for ftirther manipulating the sensor outputs, as described 
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below. One suitable processor 23 is the DSP processor Model TMS 320C50 
available from Texas Instruments, Inc. of Dallas, Texas. 
[0026] The on-board processor 23 may be controlled to perform various 
calibration and operational routines via the conventional RS-232 controller port 25. 
The processor 23 may supply output data via a Digital-to-Analog (D/A) converter 
27 for analog outputs per sensor in time-shared mode, or may provide output data 
per sensor as pulse-width modulated (PWM) output signal 29 in time-shared mode. 
[0027] As illustrated in the flow chart of Figure 3, data from all of the sensors 
9-11, 13-15, and 17 (and 18a- 18c, optionally) (Figure 2) in one processing cycle 
are converted 31 in time-sequenced, shared mode by A/D converter 21 for storage 
in selected memory locations of the EEPROM 24. This digital data is converted 33 
by the processor 23 to the sensed variables of accelerations and angular rates of the 
sensors within the assembly relative to the respective X, Y, and Z axes. This step 
is done using the calibration data stored in the EEPROM via application of the 
Sensor Compensation formulas, as set forth in Appendix A. Appendix A to 
Appendix G are provided below. A method of determining the proper calibration 
data (coefficients) in the Sensor Compensation formulas is described in detail 
below with reference to Figure 6, An implementing program in 'C or assembly 
language may be prepared in conventional manner to control a processor 23 that 
can execute solutions to the Sensor Compensation formulas at 100 Hz or faster 
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rates. A DSP processor Model TMS320C206 available from Texas Instruments 
and operating on program code written in *C' is suitable for the purpose. As stated 
above, optional magnetic sensors aligned with each of the X, Y, and Z axes may 
also be used to stabilize heading or yaw in addition to roll and pitch. The 
acceleration and rate sensors may then be calibrated 35 using calculations of the 
calibration coefficients, as detailed later herein. Figure 6 below illustrates a 
method of performing the initial or "factory" calibration for the attitude 
measurement sensor in accordance with the present invention. 
[0028] The angular rate signals are then integrated into a quatemion 
representation of attitude. The quatemion update formulas, and normalization of 
the quatemion, detailed in Appendix C, are fiirther discussed below with reference 
to Figure 7. The quatemion update 37 is the preferred method of attitude update 
because of the simplicity of the formulas and the small number (four) of variables 
that must be computed. Note that the integration is a function of not only the 
sensed angular rate but also the corrective angular rate as defined by cocx, cocy, 
cocz. The generation of the corrective angular rate terms are described below in the 
text that follows. The incorporation of corrective angular rates prevents long term 
drift and instability that would be caused by the use of rate sensors alone. After the 
quatemion is computed, it is normalized to have a magnitude of one (1). 
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[0029] Once the quaternion has been updated to reflect the instantaneous 
orientation of the platform or object, the quaternion is converted 39 to a Direction 
Cosine Matrix in preparation for computing the corrective rate signals ©ex, ©cy, 
©cz. The formulas for the Direction Cosine Matrix are set forth in Appendix D. A 
Direction Cosine Matrix (like the quaternion) also completely characterizes the 
attitude orientation of a body. The Direction Cosine Matrix, also referred to as a 
rotation matrix, is in fact a representation of the three rotations about the defined 
three axis (X,Y, and Z) needed to rotate a body fi-om a level orientation (relative to 
gravity), to the final orientation desired. Hence the relationships in Appendix D 
describe how the elements of the quaternion, which also describes such orientation, 
can be converted to the nine terms needed to define the Direction Cosine Matrix. 
The variables in Appendix D then include the definition of the Direction Cosine 
Matrix itself in the form of a 3 by 3 matrix (nine terms) do [3] [3], and the 
conversion of the four quatemion elements a, b, c and d into the Direction Cosine 
Matrix. 

[0030] The Direction Cosine matrix is used to convert the acceleration vector 
of the platform or object into level fi-ame readings that indicate 41 the true 
acceleration of the platform without tilt, as set forth in Appendix B. If, while the 
unit is sitting statically, there is a sensed level fi-ame acceleration, one of two 
things has occurred. The solution for the attitude of the platform or object has 
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drifted due to rate sensor drift, or the platform is moving linearly and accelerating 
in level frame coordinates. 

[0031] The level frame accelerations are used to generate a corrective rate 
signal 43. There are several different methods of generating corrective rate 
signals, including Kalman or other conventional adaptive filtering, fixed gain 
scheduling, or non-linear gain scheduling. The Kalman filter / adaptive approach 
is optimal but involves extreme complexity (and leads to a relatively larger-sized 
and more expensive device). The proportional gain scheduling and non-linear gain 
(fixed-rate gain) scheduling are efficient and accurate enough for most scenarios, 
as detailed in Appendix E. For a setting of proportional gain, the corrective rate 
signals ©ex is obtained by multiplication of the level frame acceleration variable 
alev[2] (obtained during factory calibration) with the variable KG. The KG value 
can be considered a gain representing the magnitude of the correction, is user 
selectable and is described below. For a setting of non-linear, or fixed-rate gain, 
the corrective rate signal cocx is obtained by multiplication of the factor (-1) with 
the variable UG for alev[2] > 0.0. The UG value can be considered a gain 
representing the magnitude of the fixed-rate correction, is also user selectable and 
is described below. Additionally, Figure 5A shows the plot of error correction for 
proportional gain schedule and Figure 5B shows the plot of error correction for 
non-linear gain schedule. 
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[0032] As a further option, the gain amount for the correction factors may also 
be adjusted by the user of the system of the present invention. This option permits 
the user to modify the system output for various reasons such as factors in the 
environment of the system, noise Hmitations, stability criteria, and the like. An 
example for making gain correction user selectable is to allow for the possibility of 
providing useful data during a rapidly maneuvering mission. There is a tradeoff 
between how much error in the gyros the algorithm can overcome with a low gain 
correction, compared to the errors induced fi-om having a high gain correction 
while experiencing large maneuvering accelerations. If the user has knowledge of 
the intensity of upcoming maneuvers or complete control of the flight profile, then 
an adapted gain correction scheme can be developed by minimizing the gain 
correction during the high acceleration portions, and increasing the gain correction 
when not accelerating. 

[0033] As further shown in Figure 3, a corrective rate for the Z (yaw) axis 
acceleration may be generated 45 based upon the measurements of the magnetic 
sensors 18a- 18c, as shown in Figure 2. Regarding gyroscopic errors attributable to 
heading or directional deviations, a GPS receiver can be used to measure heading 
of a platform that is moving and that supports the assembly of sensors 9-11,13-15, 
and 17. Another way to measure heading is to use magnetic sensors to measure 
Earth's magnetic field in the same assembly of the rate sensors and accelerometers. 
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The heading thus measured can optionally be used to stabilize the yaw axis from 
drift. This capability yields a directional gyro measurement without any additional 
cost. The formulas for stabilizing heading are detailed in Appendix F. 
[0034] Although the attitude solution is computed and stored internally as a 
quaternion and direction cosines matrix, it is most conveniently transmitted in 
Euler Angle format which is chosen because of its small size (3 variables versus 4 
for quaternion and 9 for direction cosines matrix) and because it is interpreted as a 
'real' angle about an axis. The Euler Angles are extracted 47 from the direction 
cosines matrix, as detailed in Appendix G, and are supplied via the interface 25 to 
the RS 232 bus. 

[0035] Thereafter, temperature corrections, and other corrections, can be 
digitally incorporated into the output data using correction values derived and 
stored in look-up tables in conventional manner. Specifically, for temperature 
calibration, the entire assembly may be operated at selected elevated temperatures 
to store acceleration and angular rate data at storage locations indexed by 
temperature values, and such data with associated corrections for temperature may 
thereafter be accessed from storage locations identified by the sensed operating 
temperature. 

[0036] Similarly, look-up table corrections may also be prepared for operation 
in particular applications such as conventional camera stabilization platforms in 
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which directional misalignments and non-linearities may be prepared in a look-up 
table indexed, for example, by angular orientations. 

[0037] One-time factory calibration of the entire assembly of accelerometers 
and angular rate sensors and temperature sensor is typically required to correct for 
non-linearities of response, misalignments relative to the coordinate axes, and the 
like. Specifically, conventional accelerometers commonly produce an output 
linearly proportional to the input acceleration, with an offset value. Thus: 

Voltage out = Scale Factor * Acceleration + Offset (Eq. 1) 

[0038] For calibration, known accelerations are applied in axial alignment in 
order to compute the scale factor and the offset from the resulting output voltages. 
Gravity along the Z axis provides a convenient reference, but since minor 
variations occur in gravity, it is important to determine gravity at the site of the 
calibration, and this can be accomplished very precisely by referencing standard 
formulas and handbooks that contain the exact gravitational constant for every 
longitude and latitude on Earth. A level surface on X-Y plane provides a 
convenient reference of zero (0) acceleration, thereby allowing determination of 
the offset voltage (also known as "bias"). 

[0039] Similarly, conventional angular rate sensors commonly produce an 
output voltage that is linearly proportional to the input angular rate, plus an offset. 
Thus: 
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Voltage out = Scale Factor * Angular Rate + Offset (Eq. 2) 

[0040] To calibrate such angular rate sensor, the sensor offset may be 
measured as an output voltage while the sensor is not moving (neglecting Earth's 
rotation as not significantly affecting the offset results). However, measuring scale 
factor traditionally requires an elaborate rotational table to subject the assembly to 
fixed angular rate for producing a resultant output voltage. Altematively, in 
accordance with the present invention, fi-om Equation 2, above: 

V out - Offset = Scale Factor * Rate (Eq. 3) 

J (V out - Offset ) dt = Scale Factor * J (Rate) dt (Eq. 4) 

J (V out - Offset) dt = Scale Factor * Angle (Eq. 5) 

[0041] Equations 3 and 4 above describe algebraic and linear mathematical 
operations on Equation 2, namely the subtracting of both sides of Equation 2 by the 
offset, and then performing the integration operator on both sides of Equation 4. 
Equation 5 then defines the notion that the integral of the angular rate is in fact the 
angle. Since all variables of Equation 5 are determined except for scale factor, this 
parameter of a sensor can therefore be conveniently determined and used in the 
processing to solve for angular rate, given a sensor voltage, as detailed in 
Appendix A. 
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[0042] Regarding accelerometer alignment errors, these errors represent cross 
coupling between the axes of sensitivity attributable to misalignment of an 
accelerometer with the associated coordinate axis of responsiveness. Of course, if 
each accelerometer can be precisely aligned with its associated coordinate axis, 
then misalignment error is eliminated. However, machining tolerances and 
fabrication anomalies contribute to unpredictable misalignment errors, and 
accordingly, cross axis calibration following assembly is employed to improve 
accuracy of acceleration responses. Specifically, the assembly of sensors is placed 
on each of its three axes and an output is recorded. The value of the output is 
subtracted from the true input acceleration which will be zero on two axes and IG 
on the axis exposed to gravitational field. The resulting data is organized into a 
matrix of 9 elements. This matrix is inverted in conventional manner and the result 
is a compensation matrix which converts measurements that have cross-axis 
sensitivities to true accelerations. 

[0043] Figure 6 illustrates a method of obtaining and setting the correction 
factors described above. The procedure for obtaining the correction factors will 
hereafter be referred to as "the calibration procedure." The calibration procedure is 
designed to adaptively estimate the correction factors for the six sensors in 
question. Optional magnetometer calibration is generated by taking measurements 
in a Helmhotz Coil. Calibrating a complicated six axis system is a difficult 
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prospect when considering the amount of errors that are inherently part of the 
measurement technology employed by the accelerometers and gyros. The 
difficulty lies in the fact that to estimate the error factors in any calibration 
technique requires understanding the underlying properties of each error to 
generate data that reflects the error specifically, and seldom are the errors 
uncoupled enough to isolate each separate error source with much confidence. 
[0044] The procedure developed provides a technique which estimates all the 
error parameters at once fi^om a single set of data, and generates the proper 
compensation for each error source. The technique employs an adaptive filtering 
scheme, a Kalman Filter, which processes a set of accelerometer and gyro readings 
fi:om a predefined set of maneuvers. In essence, the calibration involves obtaining 
data fi^om the sensors as the system 100 is placed on a leveled table 105, rotated 
about each axis and laid down on each face of the cube enclosing the system 100, 
and then processing the data with the Kalman Filter software which generates the 
compensation tables. As a final step the calibration compensation parameters are 
loaded into the EEPROM 24, fi-om which the DSP 23 of Figure 2 is able to obtain 
the parameters to apply the compensation to the raw accelerometer and gyro 
measurements to achieve calibrated data. The Kalman filter is a tried and true 
method of using a computer processor to estimate a set of parameters fi-om a set of 
measurement data. The measurement data can be a direct measurement of the 
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parameter, also known as a state, or a measurement of a signal that in some way 
has a mathematical relationship to the state the filter is trying to estimate. Such 
filter structure is made up of two separate components including a state model and 
a measurement model. The state model is a detailed mathematical representation 
of the states, which is used to simulate their behavior and obtain a prediction of 
what values the states will have at some time in the future. The measurement 
model derives a correction to the predicted state based on the measurement data, 
and it is this corrected prediction of the state that is referred to as the final state 
estimate. In the calibration procedure Kalman Filter, the states that are estimated 
are the sensor correction factors or calibration parameters. A complex 
mathematical model of the behavior of the parameters and their interdependence 
makes up the state model. It uses the accelerometer and gyro data fi"om the 
maneuvers to help in the prediction of the behavior of the parameters. The filter 
then infers fi-om the maneuvers themselves information that it uses as a 
measurement of the parameters. For instance, the filter can determine when it has 
arrived at a stationary point in a maneuver, and therefore assumes that 
accelerometers should measure zero with one axis along the gravity axis, and that 
the gyros should also measure zero. This information is processed by the 
measurement model, which deems any gyro data or gravity axis offset at this point 
as a measurement of the parameters, and forms a correction for the predicted 
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parameters from the state model. A list of the parameters currently estimated by 
the filter is represented in the Table 1 below. Each parameter is weighted for its 
impact on the overall performance of the filter. The marks shown below depict 
those parameters deemed important for the represented sensor. 
Table 1 





Gyros or 
Rate Sensors 


Accelerometers 


Bias 


X 


X 


Scale Factor 


X 


X 


Squared Non-Linearity 




X 


Misalignment 


X 


X 



[0045] A Kalman filter algorithm for navigating the DMU in a local level 
frame during calibration is described above. More specifically, the compensation 
parameters can be determined using a calibration procedure designed to adaptively 
estimate the calibration parameters. Calibrating a complicated sensor like the 
DMU is a difficult prospect when considering the errors that are inherently part of 
the measurement technology employed by the accelerometers and rate sensors. 
Traditional methods of calibration involve generating enough data from rate and 
vibration tables and then using curve-fitting techniques to determine the correct 
compensation. The difficulty lies in the fact that to estimate the error factors in 
any calibration technique requires understanding the underlying properties of each 
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error to generate data that reflects the error specifically, and seldom are the errors 
uncoupled enough to isolate each separate error source with much confidence. 
[0046] Thus, utilizing an adaptive calibration technique based on a Kalman 
filter involves generating data fi*om the DMU as it is placed on a leveled table, 
rotated about each axis and laid down on each face of the cube-shaped DMU 
package 100, as illustrated in Figure 6, and then processing the data with the 
Kalman filter software to generate the compensation tables. As a final step the 
calibration compensation parameters are coded into the DMU's onboard processor 
and memory for application as compensation to the raw accelerometer and rate 
sensor measurements to achieve calibrated data. 

[0047] In the Kalman filter that is applied during calibration, the states that are 
estimated are the calibration parameters and the reference measurements are the 
accelerometer and rate sensor data fi"om the maneuvers and from known static 
positions of the packaged DMU 100 on the leveled table 105. The filter then infers 
fi"om the maneuvers the information that it uses as a measurement of the 
parameters. For instance, the filter can determine when it has arrived at a 
stationary point in a maneuver, and therefore assumes that accelerometers should 
measure zero, except for the measure of 1 along the gravity axis, and that the rate 
sensors should also measure zero. This information is processed by the 
measurement model, which regards any variations fi-om these conditions in the rate 
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sensor data or gravity axis at this point as a measurement of the offset or bias 
parameters, and forms a correction for the predicted parameters from the state 
model. A list of the parameters estimated by the filter is represented in Table 1, 
above. Each parameter may be weighted for its impact on the overall performance 
of the filter. The marks shown in Table 1 depict those parameters deemed 
important for the represented sensor. 

[0048] Figure 7 illustrates a method of updating and normalizing a quaternion 
that is obtained from the sensor outputs. Appendix C defines the update and 
normalization procedures for the quaternions. The update portion is in fact a 
propagation of the quatemion vector as it propagates in time. In order to 
characterize how the quatemion changes in time, an equation is created which 
explicitly defines the time dependency of the quatemion (or its derivative), and this 
equation is referred to as a differential equation. The propagation is then obtained 
by calculating the solution to the differential equation. In order to solve the 
differential equation we need to integrate 150 the equation which then removes the 
time dependence of the derivative of the quatemion, and allows us to obtain the 
value of the quatemion at any time. In order to maintain complete stability of the 
calculation, one of the properties of the quatemion that must be maintained is that 
the magnitude (the square root of the sum of the squares) of the quatemion vector 
must always equal one (1). This is achieved by normalizing 37 the quaternions. 
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The variables in Appendix C describe both the updating and normalization process. 
The quaternion is represented by a, b, c and d, and the angular rate vector 
(calibrated gyro measurements) is represented by (oc[3]. The four temporary terms 
a4, b4, c4 and d4 represent the derivative of the quaternion, and the four equations 
for these terms represent the differential equation for the quatemion vector. Notice 
the dependence of the derivative (time rate of change) of the quatemion on the 
angular rate terms which really represent the rate of change of the rotation about 
each axis X,Y and Z. Also notice that the corrective angular rates oocx, ©cy and co 
cz are also present in the differential equation. This is in effect how the angular 
rate terms obtained from the gyros are corrected. The update quatemion section 
and the four equations using the quatemion terms define the integration of the 
differential equation process. The time dependency on the derivative of the 
quatemion is solved for, and therefore the quatemion vector itself is calculated. 
The final section then normalizes the calculated quatemion thereby assuring that 
the magnitude of the quatemion will always be one (1). 
[0049] Reference is now made to Figures 4A and 4B in order to discuss a 
further compensation process that may be provided by the system in accordance 
with the present invention. The bandwidth of conventional sensors is commonly 
too narrow (e.g. about 10 Hz) for many applications requiring quick stabilization to 
produce accurate output data rapidly. In such applications, a frequency- 
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compensating network 26, as shown in Figure 4A in either digital or analog form, 
may be introduced into the circuit to operate on sensor output signals, and may 
include a differentiator 40 and summing junction 42 followed by a low pass filter 
44. Specifically, the angular rate signal, o, is applied to differentiator 40 and to 
summing network 42 which is also connected to receive the output of the 
differentiator 40. A low-pass filter 44 having a bandwidth to about lOOH^ receives 
the combined angular rate signal, co, and the differentiated angular rate signal at the 
output of summing network 42 to provide the compensated angular rate signal, coc, 
having higher fi-equency components and wider bandwidth, as shown in Figure 4b, 
than the initial angular rate signal. 

[0050] Thus, the fi-equency compensation stage performs fi-equency 
compensation to the gyros and accelerometers which can provide enhanced 
dynamic response of, reduce the noise in, and reduce the sensitivity to vibration of 
the quaternion update. In other words, performing frequency compensation is 
performed to the gyro sensor data which expand the operational bandwidth of the 
gyros to provide updates to the quaternion under dynamic conditions which the 
gyro sensor alone would not be able to track, or which compress (filter) the gyro 
and accelerometer bandwidth to reduce noise and to reduce vibration sensitivity in 
the quaternion calculation. 
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[0051] As a further option, the user may turn off the low-pass filter 44 so that 
the above filter compensation technique is not performed. 
[0052] Referring now to the block schematic diagram of Figure 8, an 
additional slave co-processor 5 1 is included in the system to perform the Kalman 
filtering computation. As in the embodiment previously described with reference 
to Figure 2, data acquisition components include three-axes gyros 53 and three- 
axes accelerometers 55, and optionally magnetic or compass or other heading 
references (not shown), together with temperature monitor 57 and an A/D 
converter 59 under control of the main or master processor 61 that includes non- 
volatile memory for storing calibration error correction data for the data acquisition 
components, all in a manner as previously described herein. The main processor 
61 performs the attitude computation, as previously described herein, and also 
performs system control monitoring. The computed data or raw sensor data in 
digital form may be accessed through a serial communication bus (e.g., RS232) 53, 
as previously described herein, or may be accessed in analog form with error 
compensation included via a D/A converter 65 in the analog output channel 67. 
[0053] In order to run the Kalman filter efficientiy, a second or slave co- 
processor 69 is closely coupled to the main processor 61 and is used to optimally 
implement the necessary algorithms, as described in the enhanced implementation 
data herein. As illustrated in Figure 9, and as previously described herein, data in 
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the process flows from sensor array through a compensation stage, and then an 
attitude computation stage. 

[0054] At each point in the process, the sensor outputs are processed to 
produce optimum performance depending upon the apphcation and temperature 
and other environmental conditions. The optimizing parameters are determined 
during manufacturing and caHbration and are permanently stored in an EEPROM, 
as previously described herein. 

[0055] In accordance with this embodiment of the present invention, the 
attitude determination algorithm is divided into two separate entities. Information 
about angular rate as measured by rate sensors is integrated in time in an attitude 
processor routine. If the initial attitude of the vehicle was known exactly and if the 
rate sensors provided perfect readings then the attitude processing would suffice. 
However, the initial attitude is seldom known, and rate sensors typically provide 
corrupted data due to bias drift and turn-on instabihty. Without an adaptive filter 
structure such as a Kalman filter and separate independent measurements, the 
computed navigation results would diverge off the true trajectory. The Kalman 
filter attitude-correction routine therefore provides an on-the-fly calibration by 
providing corrective signals (referred to as correction quaternion terms) to the 
computed attitude processor quaternion and a characterization of the rate sensor 
bias state, with the accelerometers providing an attitude reference from gravity. 
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[0056] Referring now to Figure 10, the reference plane described above with 
respect to Figure 1 may be considered as a coordinate frame for the attitude 
measurement. The body axis of a vehicle such as an aircraft is defined as the 
coordinate frame with positive x through the vehicle nose, positive y through the 
right wing, and positive z down to complete the right-handed orientation of three 
axes. Its origin is nominally located at the vehicle center of gravity (CG). In 
Figure 10, the body frame of the vehicle is shown relative to the tangent frame or 
local level horizontal frame. In this construct, the body axis 71 is chosen to point 
toward north along its x-axis when there is no yaw angle. Therefore when the 
vehicle attitude is zero, or that the Euler angles of roll, pitch, and yaw are zero, the 
transformation from body frame to tangent frame is simply: 



CB1T = 



1 0 0 
0 1 0 
0 0 1 



(Eq. 6) 

[0057] Here the matrix represents the "Cosine rotation matrix which converts 
from Body (2) to Tangent frame". The term tangent frame can be used 
interchangeably, with the term level frame as used in the previous description. 
This serves as the starting point for further rotation of the body in the tangent 
frame due to changes in vehicle attitude. Several angles defined in the Figure 10 
are of importance for the vehicle. The centerline of the vehicle 73 is shown 
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displaced from the vehicle's velocity vector 75 in the tangent frame. The angles 
which the velocity vector 75 makes with respect to the vehicle centerline 73, are 
the typical aerodynamic control angles, including angle of attack: a, and angle of 
sideslip |3. The angles that the velocity vector makes with respect to the tangential 
plane are the typical air velocity angles including flight path angle X and heading 
angle y. The Euler body angles, which the centerline of the vehicle body 73 makes 
with respect to the tangential frame are the pitch 9 and yaw \|; angles. The vehicle 
body roll angle ^ is rotated along its centerline. The Euler angles describe tiie 
vehicle attitude and form a 3-2-1 rotation of the body in the tangent frame. In 
explicit terms the rotation matrix is: 





'1 0 


0' 


cy/ -sy/ 0 


' cO 0 sQ 
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0 1 
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cy/c^ + s\i/s6s(f> 


cy/s<l) ■>!■ sy/sOc^ 
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cdc<f> 









(Eq. 7) 



where c is abbreviation for cosine; and 
sis abbreviation for sine. 

[0058] From this rotation matrix which will transform a vector from the body 
frame into the tangent frame, the attitude Euler angles can be derived as follows: 
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^(p,.*,=-asin(C52r(3,l)). 




(Eq. 8) 




[0059] As previously described herein the attitude estimation algorithm 
processed provides stable Euler roll, pitch angles. These angles are determined by 
integrating the rate signals into a quaternion representation of attitude (or the 
CB2T matrix). The quaternion formulation for the transformation is repeated 
below. Following Euler's theorem, the quaternion is a representation of a rotation 
about an axis of rotation (e/, o-nd 63) by an angle cp. The components of the 
quaternion are: 



[0060] The quaternion vector component representing the axis of rotation is g , 
and the quatemion scalar component represents the angle of rotation. The 
cosine rotation matrix is directly defined by the quatemion as: 




(Eq. 9) 
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CB2T = 



9o + -<ll- 93 2(^,^2 - q^q^ ) 2{q,q^ + q^q^) 
2fe,4'3 - ^0^2 ) 2(^2^3 + q^q, ) ql - qf - ql + q] 



(Eq. 10) 



[0061] In this way, the quaternion that makes the transformation is: 



QB1T = 



9o 

^2 



(Eq.ll) 



[0062] Finally the body attitude is calculated from the QB2T quaternion (body 
to tangent) by employing the derivative of the quaternion formulation described 
below: 



A = \QB2T] = 



(Eq. 12) 



[0063] The quaternion equation contains a matrix representation Q, of the 
angular rate of change of the coordinate frame in question. The matrix is made up 
of the angular rates about each coordinate axis: 



0 -a>, -G>^ -co. 



Q), 



y z 



G) —CO 



0 



m = 



0), 
CO, 



CO —o> 



0 



(Eq. 13) 



Angular rate about the X-axis 
Angular rate about the Y-axis 
Angular rate about the Z-axis 
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[0064] The solid-state gyros sense the body angular rates and the differential 
equation above is integrated to obtain the propagated quaternion QB2T. The 
relationships described above then provide the cosine rotation matrix from the 
quatemion and finally the attitude angles of roll, pitch and yaw from the cosine 
rotation matrix. 

[0065] As previously described herein, a Kalman filter uses acceleration 
information, in particular the level (or tangent) frame acceleration information, to 
generate a corrective rate signal, thereby removing drift and improving accuracy of 
the attitude measurement. In another embodiment of the present invention, the 
attitude correction by the Kalman filtering achieves improved performance due to 
its ability to estimate the attitude errors and rate sensor bias (offset) states. In this 
embodiment, an absolute attitude error estimate is provided to the trajectory to 
correct any errors due to physical noise disturbances and rate sensor errors, as well 
as a characterization and "tracking" of the rate sensor biases which in effect 
provide an online rate sensor calibration. The filter model in this embodiment is an 
Extended Kalman Filter formulation made up of a linearized attitude error and rate 
sensor bias state model, and a nonlinear attitude quatemion error measurement 
model. The state model predicts where the attitude errors and rate sensor bias 
states will propagate based on input data from the rate sensors, and the 
measurement model corrects this prediction with the real world attitude error 
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measurements obtained from the accelerometer gravity reference (also referred to 
as level frame acceleration), and from the leveled magnetometer heading reference. 
This balance of state modeling with real world parameters gives the Kalman filter 
the adaptive intelligence to assign appropriate confidence levels on its two 
components. 

[0066] In this embodiment, the Kalman filter is designed to correct the 
trajectory calculated by the attitude processor 61 and its state space is confined to 
estimating errors or perturbations in that attitude trajectory due to corrupt sensors. 
This is referred to as generating a trajectory perturbation state vector which 
includes attitude perturbations and sensor characterization that model the absolute 
error sources in the sensors, composed mainly of the rate sensor biases, or offsets. 
[0067] The attitude perturbation is defined to be the error in the attitude 
processor knowledge of the body attitude with respect to the tangent frame. The 
filter state elements and their sizes are defined as follows: 



3*1 



-5q' 

[0068] There are three rate sensor bias estimates for the rate sensors on 



(Eq. 14) 

3x1 



each axis in the veliicle body frame. The attitude perturbation is modeled as a 
quatemion and is similar to Eq. 9. It defines the perturbation in vehicle attitude 
and is used to update the body-to-tangent frame cosine rotation matrix CB2T, thus 
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providing a means to correct or "rotate" the current body frame to the latest 
estimate for the body frame. 



'8q, 
5q= Sq^ 



dq = 



8q^ 



(Eq. 15) 



[0069] In presenting the state transition matrix below, the matrix cross product 
formulation is employed throughout. The definition of the notation is as follows: 





0 








M X V = [mx]v = 




0 












0 





[0070] 



(Eq. 16) 

The attitude perturbation model contains most of the dynamical 



information, and therefore the attitude error reference which is obtained from the 
accelerometers and which provides a clear measure of the vehicle attitude in the 
tangent frame is significant. The rate sensor signals are used directly in the state 
transition matrix making it a time-varying process, as well as making it sensitive to 
the quality of the sensor signals. Very noisy rate sensor measurements should be 
pre-filtered, as previously discussed herein, before introducing them into the 
system, especially if that noise is electronic or high-frequency vibration 
disturbances above the bandwidth of the vehicle performance. The matrix is 
presented in the relationship below: 
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(Eq. 17) 











[;.] 



















[0071] From the perturbation state dynamics defined above, certain clear 

relationships are formed. The attitude perturbation dq dynamics are affected by 

errors due to coupled sensors in the rate sensor, with absolute errors in attitude that 

explain its dependency on the rate sensor measurements as well as the rate sensor 

bias terms. Each term is defined below: 

= Gyro,^^ *5q'-5q'* Gyro,^ - \{Gyro,^ + 0.5* (2* Sqx Gyro^ + *dg, ))x] ^ 
A^, =0.5* I, -[Sq>q ^' ^ 

[0072] Using a quaternion formulation for a^ti^ade perturbations pro\ddes the 
advantage of using the higher order terms of the quaternion, and improved 
dynamic behavior of the quaternion. Perturbations in Euler angles, and its 
dynamic propagation is a very simple first order approximation that breaks down if 
the attitude errors grow large. The state process noise terms dy defined in the state 
dynamic equations (Eq. 17) are modeled as white noise for all the states. The noise 
covariance matrix for the state transition noise terms is labeled Q and is defined as 
a diagonal 6x6 matrix with values initially chosen to achieve the desired filter 
performance fi-om flight trajectory simulation. 

[0073] In order to improve the attitude error estimation portion of the Kalman 
filter, an adaptive component was added to the State Model Noise Covariance 



34 

1 8856/08206/DOCS/l 383593. 1 



which allowed the filter to respond very quickly to situations in which the 
reference information is probably corrupted. Two dynamic conditions cause the 
accelerometer reference of gravity to be corrupted, including: 

1 . Dynamic acceleration changes due to change-in-velocity maneuvers; and 

2. Coordinated Turn Maneuvers (or tums generating significant centripetal 
acceleration). 

[0074] In the first condition, the accelerometers measure the true body 
acceleration due to a change-in-velocity, which will corrupt its measurement of 
gravity and can lead to a larger than true attitude reference. In the second 
condition, flie accelerometers measure the combined (null) effect of gravity and 
centripetal acceleration, which leads to a smaller than true attitude reference. The 
goal then is to determine when these situations arise and to provide the proper 
adaptive change to the State Model Noise Covariance. The two components that 
make up this model are the Dynamic Maneuver Determination Test, and the 
Coordinated Turn Determination Test, as illustrated in the flow charts of Figures 
1 1 and 12. The Dynamic Maneuver Determination Test is based upon the 
acceleration data obtained fi:om the accelerometers. As defined in the flow 
diagram of Figure 11, the three vector accelerations 81 are converted into a 
lateral/longitudinal magnitude of acceleration term 83 which excludes the Z- 
acceleration component. This magnitude signal is then passed through a high-pass 
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filter 85 to remove any bias in the signal. The resultant signal provides a measure 
of the amount of "maneuvering" acceleration sensed by the accelerometers in the 
X-Y plane. This is the plane of reference used to obtain the attitude with respect to 
gravity information used by the Kalman filter. The signal is then checked against a 
threshold value 87 and, if in excess of the threshold value 89, the algorithm 
assumes that the vehicle is maneuvering and will no longer accept the attitude 
computation as correct with respect to gravity reference. 

[0075] The Coordinated Tum Maneuvers Test is based solely on the yaw gyro 
signal 91 which is first passed through low-pass filter 93, as shown in Figure 12, to 
remove typical yaw tum perturbations due to small flight or body corrections. The 
signal is then checked against a threshold 95 and, if in excess of the threshold 97, 
the algorithm assumes the vehicle is in a coordinated tum 99 or other tum 
maneuver that generates significant centripetal force. The algorithm accordingly 
lowers the State Model Noise Govariance, which effectively lowers the gain and 
response of the Kalman filter. With the Kalman filter in a low-gain mode, attitude 
error build-up during a tum is kept to a minimum by maintaining those gyro bias 
estimates that were produced prior to the tum maneuver, and lowering the 
weighting on the accelerometer attitude reference which would otherwise attempt 
to force the attitude indication toward level. 
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[0076] The measurement model for the filter contains components that are 
nonlinear in nature. The attitude reference error measurements are direct 
measurements of the Euler angle errors, and are tiierefore a nonlinear combination 
of the quatemion perturbation states. They are obtained by rotating the body 
measured accelerometer signals into the tangent or level fi-ame, and then 
calculating the attitude reference error by observing any residual non-level 
acceleration terms in the tangent and assigning the attitude error measurements 
based on the magnitude of the residual terms. 

[0077] The heading reference error is obtained by calculating the heading 
direction as measured from the leveled magnetometers, and comparing this 
heading to the current calculated yaw angle obtained from the attitude trajectory 
that is determined from the integrated quatemion. The heading error measurement 
then is based on the magnitude of the residual angle error difference between the 
magnetometer heading and the calculated yaw. The following relationship 
describes the Kalman filter measurement model: 
[0078] The reference residuals comprise the body accelerometer and 
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(Eq. 19) 
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magnetometer data and are obtained from the following relationship: 



A CCj-^f/ — CB2T * A CCgQDY 



ACCX. 



TAN residual 



^^^^TAN residual 



^(ADAPT) 
— Sig^(ACCj-^i^Y ) * i^(ADAPT) 

sigftiHeading^j^ ) * vr^^DAPT) . 
Heading ^^j, = - Heading 
Heading = atan 2{MA GYj.^^ , MA GXj.^,^ ) 



(Eq. 20) 



MAGX. 



TAN 



MAGY, 



TAN 



'cos{d)*MAGx +sin(^)*sin(<^)*M.4Gy +cos{<fi)*MAG2 
sm{<l>)*MAG2 -cos{</>)* MAGy 



[0079] The angle perturbation estimate functions represent the attitude error 
that the Kalman filter has estimated up to that point, and are obtained from the 
following relationship: 



taia = ].0-2.0*(Sq2 * Sq^ +<5q, *^,; 
tan3 = 2.0*('(5q, *<5q2 +Sq3*SqJ 
tan4 = L0-2.0*(&i2 *<5q2 +<5q3 
atan2( tanl.tanl ) 



[0080] 



atan2(tm3,tm4) 
The measurement matrix Cmeas in Eq. 19 is calculated as: 



(Eq. 21) 
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and is the linearized matrix representation that essentially transforms the Kalman 
filter estimated quaternion perturbation into the Kalman filter measurement attitude 
reference error. 

[0081] Measurement noise covariances are defined fi-om the performance of 
the accelerometers and magnetometers and are contained in R„,„,in Eq. 19. 
[0082] A conventional Kalman filter is employed and its function and typical 
nomenclature for coefficients, and the like, are described below. The filter state 
and transition matrix is first discretized, and then the discrete time Kalman filter is 
employed. Since the measurements come in at varying sample rate, a varying-rate 
filter configuration is employed. 

[0083] During an update, the full Kalman filter is calculated which includes the 
prediction and correction steps. The equations follow: 

A, = (I + At* A,, J 
H = A,*P„,*Aj+Q, 

K = H*C„J *(C_ *H*C„J+R„^r ■ (Eq. 23) 

^ — (ymeas ~ ^meas ^pred) 

[0084] The measurement matrix Cmeas is defined above in Eq. 22, and the state 
transition matrix Astate^s defined in Eqs. 17 and 18, and the measurement noise 
covariance R„,eas is defined above in Eq. 19. The measurement vector 3;;;,ea, is 
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defined from the attitude reference error calculation described in the measurement 
model discussion above and is represented in Eq. 19. The state process noise 
covariance Qk is a discretized form of the continuous state process noise 
covariance Q to account for the varying filter sample rate. Each element of the 
Kalman filter equations of Eq. 23 have been defined previously, and the interim 
terms such as H, and K and Pgst are conventional in classic Kalman filter 
terminology. 

[0085] Therefore, the apparatus and processes according to the present 
invention provide a low-cost sensor assembly for platform stabilization systems 
and for other dynamic positioning and guidance control systems in automotive, 
marine, and aeronautical applications that require convenient gravity reference. 
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Appendix A - Sensor Compensation 

Raw Sensor Inputs 

ab[3] Accelerations uncorrected (ab[l] = x, ab[2] = y, abl[3] = z 
a)b[3] Rate Sensor uncorrected (a)b[l ] = x, o)b[2] = y, a)b[3] = z) 

Corrected Sensor Outputs 
ac[3] Accelerations corrected 
a)c[3] Rate Sensor corrected 

Calibration coefficients 
ba[3] accelerometer bias 
sa[3] accel scale error 
L2[3] accel non-linearity 
bo)[3] rate bias 
S(o[3] rate scale error 
ma[12] misalignment table 

Accelerometer Channel Error 
al[l] = ((L2[l]*ab[l]+sa[l]*ab[l]+ba[l])+ab[l] 
al[2] = ((L2[2]*ab[2]+sa[2]*ab[2]+ba[2])+ab[2] 
al[3] = ((L2[3]*ab[3]+sa[3]*abr31+ba[3])+ab[3] 

Gyro Channel Error 
Q)l[l]=(sa)[l]* (ob[l]+bo)[l]+ a)b[l] 
a)l[2]=(sa)[2]* a)b[2]+ba)[2]-h o)b[2] 
a)l[3]=(so)[3]* o)b[3]+bo)[3]+ o)b[3] 

Accelerometer Compensation for Misalignment 
' ac[l] =ma[l]*al[2]+ma[2]*al[3]+al[l]; 
ac[2] = ma[4]*al[l]+ma[3]*al[3]*al[2]; 
ac[3] = ma[5]*al[l]+ma[6]*al[3]+al[3]; 

Gyro compensation for misalignment 

coc[l] = ma[7]* (ol[2] + ma[8]* q)1[3] + (ol[l] 
Q)c[2] = ma[10]* o)l[l] + ma[9]* co [3] + o)l[2] 
coc[3] = ma[l 1]* col[I] + ma[12]* col [2] + col [3] 
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Appendix B - Factory Calibration 



alev[3] = level frame acceleration / tilt of platform 

ac[3] = compensated body frame acceleration readings see Step 2 

dc[3][3] = direction cosines matrix 

alev[l] = dc[l][l] * ac[l] + dc[l][2] * ac[2] + dc[l][3]*ac[3]; 
alev[2] = dc[2][l] * ac[l] + dc[2][2] * ac[2] + dc[2][3]*ac[3]; 
alev[3] = dc[3][l] * ac[l] + dc[3][2] * ac[2] + dc[3][3]*ac[3]; 
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Appendix C - Update Quaternion and Normalize Quaternion 

DT = Time change in seconds from last update divided by 2 

DQ = Normalization coefficient 

a4,b4,c4,d4 = temporary quaternion 

a,b,c,d = quaternion representation of attitude 

©c[3] = angular rate sampled over DT interval by A/D converter (1,2,3 = X,Y,Z respectively) 

(OCX = corrective rate signals x generated in Appendix A, para. 7 
©cy = corrective rate signals y generated in Appendix A, para. 7 
(Dcz = corrective rate signal z (azimuth) 

/♦compute temporary quaternion as a function of sensed rates and previous quaternion */ 

a4 = DT* ((coc[3]+cocz)*b - (coc[2]+a)Cy)*c + (-l*(oc[l]+a)Cx)*d); 
b4 = DT* ((-l*a)c[3]- cocz)*a + coc[l]+a)Cx)*c + (cocy - coc[2]*d); 
c4 = DT* (((oc[2]+ (ocy)*a - ((oc[l]+cocx)*b + (-l*a)c[3]+a)cz)*d); 
d4 = DT* ((a>c[l]- a)cx)*a + ((Dc[2]-(Dcy)*b + ((dc[3]-(ocz)*c); 

/* update quaternion*/ 

a += a4; 

b+=b4; 

c += c4; 

d+=d4; 

/* normaUze quaternion */ 

DQ = 1 - 05*(a*a + b*b + c*c +d*d -1); 

a *= DQ; 

b*=DQ; 

c*=DQ; 

d*=DQ; 
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A ppendix D - Convert Quaternion to Direction Cosines Matrix 



dl,d2,d3,cl I,cl2,cl3,cl4,c22,c23,c24,c33,c34 Temporary storage 
dc[3][3] 3x3 direction cosines matrix 

/* compute temporary storage */ 

dl = a + a; 
d2 = b + b; 
d3 = c +c; 

cl 1 = dl * a; 

cl2 = dl *b; 
cl3=dl *c; 
cl4 = dl *d; 
c22 = d2*b; 
c23 = d2 * c; 
c24 = d2 * d; 
c33 = d3 * c; 
c34 = d3 * d; 

/* update direction cosines */ 
dc[l][l] = l-c22-c33; 
dc[2][2] = l-cll-c33; 
dc[3][3] = l-cl 1 -022; 
dc[2][l] = cl2-c34; 
dc[l][3] = cl3-c24; 
dc[3][2] = c23-cl4; 
dc[l][2] = cl2 + c34; 
dc[3][l] = cl3 + c24; 
dc[2][3] = c23 + cl4; 
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Appendix E - Generate Corrective Rates 

Option 1 : Proportional Gain 
©cx = - KG * alev[2]; 
ci)cy = KG*alev[l]; 

Option 2: Non-linear Gain 

if (alev[2] > 0.0) 

a)Cx = -l*UG 

else 

©cx = UG 

if(alex[l]>0.0) 
©cy = UG; 

else 

®cy = -l*UG 



Appendix F - Heading /Azimuth Corrective Rate 

©cz: azimuth corrective rates 
ct: cosine of true heading 
St: sine of true heading 

©cz = KG * (ct*dc[2][l] - st*dc[l][l]) 



Appendix G - Extract Euler Angle for Direction Cosines 

Roll Jitch, Yaw - final outputs in Euler Angle format 

Roll = atan (dc[3][2]/dc[3][3]) 

Pitch = asin (dc[3][l]) 

Heading = atan (dc[3][2], dc[l][l]) 



1 8856/08206/DOCS/l 383593. 1 



